#ifndef VISIONNODE_H_
#define VISIONNODE_H_

#include <ros/ros.h>
#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/image_encodings.h>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>

#include "std_msgs/String.h"
#include <sensor_msgs/Image.h>
#include <opencv/cv.h>
#include <opencv/highgui.h>


#include <boost/thread.hpp>
namespace std {
using boost::mutex;
using boost::condition_variable;
using boost::unique_lock;
using boost::thread;
}

#include "../utilities/Constants.h"


namespace enc = sensor_msgs::image_encodings;
using namespace cv;


class VisionNode {

private: 
	ros::NodeHandle m_node_handler;
	// ros::NodeHandle m_rgb_node_handler;
	// ros::NodeHandle m_depth_node_handler;
	image_transport::ImageTransport m_rgb_trans;
	// image_transport::ImageTransport m_depth_trans;

	// subscribers
	ros::Subscriber m_rgb_sub;
	ros::Subscriber m_depth_sub;

	// publishers
	image_transport::Publisher m_image_pub;

	ros::Publisher m_obj_pos_publisher;
	std_msgs::String m_message_to_publish;

	std::mutex mutex_;	

	void safeObjPosSet(int x, int y, double newDist);

public:
	VisionNode();
	virtual ~VisionNode();

	void calcDistCB(const sensor_msgs::Image::ConstPtr& msg);
	void detectObjCB(const sensor_msgs::Image::ConstPtr& msg);

};

#endif /* VISIONNODE_H_ */
